;;; Project 1, 6.001, Spring 2005

;;; idea is to simulate a baseball robot

;; imagine hitting a ball with an initial velocity of v 
;; at an angle alpha from the horizontal, at a height h
;; we would like to know how far the ball travels.

;; as a first step, we can just model this with simple physics
;; so the equations of motion for the ball have a vertical and a 
;; horizontal component

;; the vertical component is governed by
;; y(t) = v sin alpha t + h - 1/2 g t^2 
;; where g is the gravitational constant of 9.8 m/s^2

;; the horizontal component is governed by
;; x(t) = v cos alpha t
;; assuming it starts at the origin

;; First, we want to know when the ball hits the ground
;; this is governed by the quadratic equation, so we just need to know when 
;; y(t)=0 (i.e. for what t_impact is y(t_impact)= 0).
;; note that there are two solutions, only one makes sense physically

(define square
  (lambda (x) (* x x)))

;; these are constants that will be useful to us
(define gravity 9.8)  ;; in m/s
(define pi 3.14159)

;; Problem 1

(define position
  (lambda (a v u t)
    (+
     (* 0.5 a (square t))
     (* v t)
     u)
    ))

;; you need to complete this procedure, then show some test cases

; (position 0 0 0 0); => 0.0
; (position 0 0 20 0); => 20.0
; (position 0 5 10 10); => 60.0
; (position 2 2 2 2); => 10.0
; (position 5 5 5 5); => 92.5
; (position 2 2 2 0); => 2.0
; (position 0 5 5 5); => 30.0


;; Problem 2

(define root1
  (lambda (a b c)
    (let ((Delta (- (square b) (* 4 a c))))
      (if (< Delta 0)
	  (error "no real root")
	  (/
	   (+ (* -1 b) (sqrt Delta))
	   (* 2 a)
	   )
	  )
      )
    ))

(define root2
  (lambda (a b c)
    (let ((Delta (- (square b) (* 4 a c))))
      (if (< Delta 0)
	  (error "no real root")
	  (/
	   (- (* -1 b) (sqrt Delta))
	   (* 2 a)
	   )
	  )
      )
    ))

(root1 5 3 6);ERROR: no real root
(root1 1 -2 1);=> 1.0
(root2 1 -2 1);=> 1.0
(root1 1 5 6);=> -2.0
(root2 1 5 6);=> -3.0

;; complete these procedures and show some test cases

;; Problem 3

(define time-to-impact
  (lambda (vertical-velocity elevation)
    (root2 (* -0.5 gravity) vertical-velocity elevation)
    ))

;Because coefficient a is negative, so root2 > root1.
(time-to-impact 1.2 3.7);=>1.0
(time-to-impact gravity gravity); => 2.73205080756888


;; Note that if we want to know when the ball drops to a particular height r 
;; (for receiver), we have

(define time-to-height
  (lambda (vertical-velocity elevation target-elevation)
    (time-to-impact vertical-velocity 
		    (- elevation target-elevation))
    ))

(time-to-height 1.2 5 1.3);=> 1.0

;; Problem 4

;; once we can solve for t_impact, we can use it to figure out how far the ball went

;; conversion procedure
(define degree2radian
  (lambda (deg)
    (/ (*  deg pi) 180.)))

(define travel-distance-simple
  (lambda (elevation velocity angle)
    (let* ((radian (degree2radian angle))
	  (t (time-to-impact (* velocity (sin radian)) elevation)))
      (* t (* velocity (cos radian)))
	  )
    ))

(travel-distance-simple 5 10 45); => 13.8799275977916

;; let's try this out for some example values.  Note that we are going to 
;; do everything in metric units, but for quaint reasons it is easier to think
;; about things in English units, so we will need some conversions.

(define meters-to-feet
  (lambda (m)
    (/ (* m 39.6) 12)))

(define feet-to-meters
  (lambda (f)
    (/ (* f 12) 39.6)))

(define hours-to-seconds
  (lambda (h)
    (* h 3600)))

(define seconds-to-hours
  (lambda (s)
    (/ s 3600)))

;; what is time to impact for a ball hit at a height of 1 meter
;; with a velocity of 45 m/s (which is about 100 miles/hour)
;; at an angle of 0 (straight horizontal)
;; at an angle of (/ pi 2) radians or 90 degrees (straight vertical)
;; at an angle of (/ pi 4) radians or 45 degrees
(travel-distance-simple 1 45 0);=>20.3289278153682
(travel-distance-simple 1 45 90); => 5.49641898961246e-4
(travel-distance-simple 1 45 45); => 207.627861151491

;; what is the distance traveled in each case?
;; record both in meters and in feet


;; Problem 5

;; these sound pretty impressive, but we need to look at it more carefully

;; first, though, suppose we want to find the angle that gives the best
;; distance
;; assume that angle is between 0 and (/ pi 2) radians or between 0 and 90
;; degrees

(define alpha-increment 0.01)

(define find-best-angle
  (lambda (velocity elevation)
    (let ((best-angle 0) (longest-distance 0) (angle 0))
      (define (computer-distance angle)
	(if (> angle 90)
	    best-angle
	    (let ((distance (travel-distance-simple 
			     elevation velocity angle)))
	      (if (> distance longest-distance)
		  (begin
		   (set! longest-distance distance)
		   (set! best-angle angle)
		   )
		  )
	      (computer-distance (+ angle 1))
	      )
	    )
	)
      (computer-distance 0)
      )
    ))

(find-best-angle 5 10); =>19
(find-best-angle 10 5); =>35
(find-best-angle 10 10); =>30
;; find best angle
;; try for other velocities
;; try for other heights

;; Problem 6

;; problem is that we are not accounting for drag on the ball (or on spin 
;; or other effects, but let's just stick with drag)
;;
;; Newton's equations basically say that ma = F, and here F is really two 
;; forces.  One is the effect of gravity, which is captured by mg.  The
;; second is due to drag, so we really have
;;
;; a = drag/m + gravity
;;
;; drag is captured by 1/2 C rho A vel^2, where
;; C is the drag coefficient (which is about 0.5 for baseball sized spheres)
;; rho is the density of air (which is about 1.25 kg/m^3 at sea level 
;; with moderate humidity, but is about 1.06 in Denver)
;; A is the surface area of the cross section of object, which is pi D^2/4 
;; where D is the diameter of the ball (which is about 0.074m for a baseball)
;; thus drag varies by the square of the velocity, with a scaling factor 
;; that can be computed

;; We would like to again compute distance , but taking into account 
;; drag.
;; Basically we can rework the equations to get four coupled linear 
;; differential equations
;; let u be the x component of velocity, and v be the y component of velocity
;; let x and y denote the two components of position (we are ignoring the 
;; third dimension and are assuming no spin so that a ball travels in a plane)
;; the equations are
;;
;; dx/dt = u
;; dy/dt = v
;; du/dt = -(drag_x/m + g_x)
;; dv/dt = -(drag_y/m + g_y)
;; we have g_x = - and g_y = - gravity
;; to get the components of the drag force, we need some trig.
;; let speeed = (u^2+v^2)^(1/2), then
;; drag_x = - drag * u /speed
;; drag_y = - drag * v /speed
;; where drag = beta speed^2
;; and beta = 1/2 C rho pi D^2/4
;; note that we are taking direction into account here

;; we need the mass of a baseball -- which is about .15 kg.

;; so now we just need to write a procedure that performs a simple integration
;; of these equations -- there are more sophisticated methods but a simple one 
;; is just to step along by some step size in t and add up the values

;; dx = u dt
;; dy = v dt
;; du = - 1/m speed beta u dt
;; dv = - (1/m speed beta v + g) dt

;; initial conditions
;; u_0 = V cos alpha
;; v_0 = V sin alpha
;; y_0 = h
;; x_0 = 0

;; we want to start with these initial conditions, then take a step of size dt
;; (which could be say 0.1) and compute new values for each of these parameters
;; when y reaches the desired point (<= 0) we stop, and return the distance (x)

(define drag-coeff 0.5)
(define density 1.25)  ; kg/m^3
(define mass .145)  ; kg
(define diameter 0.074)  ; m
(define beta (* .5 drag-coeff density (* 3.14159 .25 (square diameter))))

(define integrate
  (lambda (x0 y0 u0 v0 dt g m beta)
;;use integrate-time for maintaining easily
    (car (integrate-time x0 y0 u0 v0 dt g m beta))
    ))
;;     (define (computer-speed ut vt)
;;       (let ((speed (sqrt (+ (square ut) (square vt)))))
;; 	(if (<= y0 0)
;; 	    x0
;; 	    (begin
;; 	      (set! x0 (+ x0 (* ut dt)
;; 			  (* (/ (* speed beta ut) m) dt dt -0.5)
;; 			  ))
;; 	      (set! y0 (+ y0 (* vt dt)
;; 			  (* (+ (/ (* speed beta vt) m) g) dt dt -0.5)
;; 			  ))
;; 	      (computer-speed 
;; 	       (- ut (* (/ (* speed beta ut) m) dt))
;; 	       (- vt (* (+ (/ (* speed beta vt) m) g) dt)))
;; 	      )
;; 	    )
;; 	)
;;       )
;;     (computer-speed u0 v0)
;;    ))

(define travel-distance
  (lambda (elevation velocity angle)
    (integrate 0 elevation 
	       (* velocity (cos (degree2radian angle)))
	       (* velocity (sin (degree2radian angle)))
	       0.1 gravity mass beta)
    )
  )

;; RUN SOME TEST CASES
(travel-distance 1 45 0);=>20.3650272692268
(travel-distance 1 45 90); => 2.35613146505791e-4
(travel-distance 1 45 45); => 90.3784547054408

;; what about Denver?

;; Problem 7

(define integrate-time
  (lambda (x0 y0 u0 v0 dt g m beta)
    (let ((t 0))
      (define (computer-speed ut vt)
	(let ((speed (sqrt (+ (square ut) (square vt)))))
	  (if (and (<= y0 0) (<= vt 0))
	      (cons x0 t)
	      (begin
		(set! x0 (+ x0 (* ut dt) 
			    (* (/ (* speed beta ut) m) dt dt -0.5)
			    ))
		(set! y0 (+ y0 (* vt dt)
			    (* (+ (/ (* speed beta vt) m) g) dt dt -0.5)
			    ))
		(set! t (+ t dt))
		(computer-speed 
		 (- ut (* (/ (* speed beta ut) m) dt))
		 (- vt (* (+ (/ (* speed beta vt) m) g) dt)))
		)
	      )
	  )
	)
      (computer-speed u0 v0)
      )
    ))

(travel-distance-simple 1 45 0);=>20.3289278153682

(integrate-time 0 1 45 0 0.01 gravity mass 0);=>(20.7 . 0.46)

(define find-best-angle2
  (lambda (velocity elevation distance)
    (let ((best-angle 0) (shortest-time 9999) (angle 0))
      (define (computer-time angle)
	(if (> angle 90)
	    best-angle
	    (let ((distance-time
		   (integrate-time
		    0
		    elevation 
		    (* velocity (cos (degree2radian angle)))
		    (* velocity (sin (degree2radian angle)))
		    0.01 gravity mass beta)))
	      (if (< (abs (- (car distance-time) distance))
		     1)
		  (if (< (cdr distance-time) shortest-time)
		      (begin
			(set! shortest-time (cdr distance-time))
			(set! best-angle angle)
			)
		      )
		  )
	      (computer-time (+ angle 1))
	      )
	    )
	)
      (computer-time -90)
      )
    ))
 
;; now let's turn this around.  Suppose we want to throw the ball.  The same
;; equations basically hold, except now we would like to know what angle to 
;; use, given a velocity, in order to reach a given height (receiver) at a 
;; given distance

;; a cather trying to throw someone out at second has to get it roughly 36 m
;; (or 120 ft) how quickly does the ball get there, if he throws at 55m/s,
;;  at 45m/s, at 35m/s?

;;More accuracy, more time, so I decrease accuracy by increase
the difference of desired distance and actual distance and
decrease the time step and angle step.

(find-best-angle2 55 2 36);=>1
(find-best-angle2 45 2 36);=>3
(find-best-angle2 35 2 36);=>7

;; try out some times for distances (30, 60, 90 m) or (100, 200, 300 ft) 
;; using 45m/s
(find-best-angle2 45 2 30);=>1
(find-best-angle2 45 2 60);=>11
(find-best-angle2 45 2 90);=>28
(find-best-angle2 35 2 90);=>0
;; Problem 8
(define travel-distance-with-bounce
  (lambda (elevation velocity angle bounce-time)
    (define (travel-with-bounce elevation velocity time distance)
      (let ((t-distance
 	     (integrate 0 elevation 
			(* velocity (cos (degree2radian angle)))
			(* velocity (sin (degree2radian angle)))
			0.1 gravity mass beta)))
	(if (= t-distance 0)
	    distance
	    (if (>= time bounce-time)
		(+ distance t-distance)
		(travel-with-bounce 0 (* 0.5 velocity) (+ time 1) (+ distance t-distance))
		)
	    )
	)
      )
    (travel-with-bounce elevation velocity 0 0)
    )
  )

(travel-distance 1 45 0);=>20.3650272692268
(travel-distance-with-bounce 1 45 0 0);=> 20.3650272692268
(travel-distance-with-bounce 1 45 0 1);=> 20.3650272692268
(travel-distance 1 45 45); => 90.3784547054408
(travel-distance-with-bounce 1 45 45 0);=> 90.3784547054408
(travel-distance-with-bounce 1 45 45 1);=> 128.686128940992
(travel-distance-with-bounce 1 45 45 2);=> 140.601995008125
(travel-distance-with-bounce 1 45 45 100);=> 145.548722851741
(travel-distance-with-bounce 1 45 45 101);=> 145.548722851741

;; Problem 9

(define integrate-time-velocity
  (lambda (x0 y0 u0 v0 dt g m beta)
    (let ((t 0))
      (define (computer-speed ut vt)
	(let ((speed (sqrt (+ (square ut) (square vt)))))
	  (if (and (<= y0 0) (<= vt 0))
	      (list x0 t ut vt)
	      (begin
		(set! x0 (+ x0 (* ut dt) 
			    (* (/ (* speed beta ut) m) dt dt -0.5)
			    ))
		(set! y0 (+ y0 (* vt dt)
			    (* (+ (/ (* speed beta vt) m) g) dt dt -0.5)
			    ))
		(set! t (+ t dt))
		(computer-speed 
		 (- ut (* (/ (* speed beta ut) m) dt))
		 (- vt (* (+ (/ (* speed beta vt) m) g) dt)))
		)
	      )
	  )
	)
      (computer-speed u0 v0)
      )
    ))

(define travel-distance-with-bounce2
  (lambda (elevation velocity angle bounce-time)
    (define (travel-with-bounce elevation ut vt time distance)
      (let ((tmp
 	     (integrate-time-velocity 0 elevation ut vt
			0.1 gravity mass beta)))
	(if (= (car tmp) 0)
	    distance
	    (if (>= time bounce-time)
		(+ distance (car tmp))
		(travel-with-bounce 0 (* 0.5 (caddr tmp)) (* -0.5 (cadddr tmp)) (+ time 1) (+ distance (car tmp)))
		)
	    )
	)
      )
    (travel-with-bounce elevation 
			(* velocity (cos (degree2radian angle)))
			(* velocity (sin (degree2radian angle)))
			0 0)
    )
  )

;;Based on my understanding, I assume velocity in horizotal would drop by one half and
;;velocity in vertical would countermove and drop by one half.

(travel-distance-with-bounce2 1 45 45 0);=> 90.3784547054408
(travel-distance-with-bounce2 1 45 45 1);=> 100.867553322969
(travel-distance-with-bounce2 1 45 45 2);=> 103.375673734667
